/******************************************************************************
  XYXKC_CAR.cpp
  xiaoyingxing line patrol car extension file
  Creation Date: 10-25-2023
  @ XYXKC

  Here, you can use it to control the travel and inspection of the line inspection car

  Distributed as-is; no warranty is given.
******************************************************************************/
#include"XYXKC_CAR.h"
#include "Arduino.h"
#include<math.h>

XYXKC_CAR::XYXKC_CAR() {}

/**
 * @brief 初始化
 * @param leftMotor 左电机
 * @param rightMotor 右电机
 * */
void XYXKC_CAR::motor_init(uint8_t *leftMotor, uint8_t *rightMotor) {
    _leftMotor = leftMotor;
    _rightMotor = rightMotor;
    if (yfLS.begin() == false) {
        while (1); // If we fail to communicate, loop forever
    }
    yfLS.enableSensor();// 使能传感器，默认使能
    stopOfPatrolCar();
}

/**
 * @brief 电机运行
 * @param about 左右电机
 * @param direction 正反转
 * @param speed 速度
 * */
void XYXKC_CAR::motorRun(bool about, bool direction, int speed) {
    speed = abs(speed) > 100 ? 100 : abs(speed);
    uint8_t *aboutMotor = about == true ? _rightMotor : _leftMotor;
    if (direction) {
        analogWrite(aboutMotor[0], 0);
        analogWrite(aboutMotor[1], speed * 10.23);
    } else {
        analogWrite(aboutMotor[1], 0);
        analogWrite(aboutMotor[0], speed * 10.23);
    }

}

/**
 * @brief 小车前进
 * @param speed 速度
 * */
void XYXKC_CAR::trolleyForward(int speed) {
    motorRun(Left, back, speed);
    motorRun(Right, just, speed);
}

/**
 * @brief 小车后退
 * @param speed 速度
 * */
void XYXKC_CAR::trolleyBackward(int speed) {
    motorRun(Left, just, speed);
    motorRun(Right, back, speed);
}

/**
 * @brief 小车左转
 * @param speed 速度
 * */
void XYXKC_CAR::carTurningLeft(int speed) {
    motorRun(Left, just, speed);
    motorRun(Right, just, speed);
}

/**
 * @brief 小车右转
 * @param speed 速度
 * */
void XYXKC_CAR::carTurningRight(int speed) {
    motorRun(Left, back, speed);
    motorRun(Right, back, speed);
}

/**
 * @brief 小车停
 * */
void XYXKC_CAR::stopOfPatrolCar() {
    motorRun(Left, just, 0);
    motorRun(Right, just, 0);
}

/**
 * @brief 巡线
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * */
void XYXKC_CAR::_PatrolLine(int speed, float proportion1, float proportion2) {
    float differentialSpeed1 = speed - speed * proportion1;
    float differentialSpeed2 = speed - speed * proportion2;
    if (yfLS.readSensor(1)) {
        motorRun(Left, differentialSpeed1 < 0 ? just : back, differentialSpeed1);
        motorRun(Right, just, speed + speed * proportion1);
    } else if (yfLS.readSensor(3)) {
        motorRun(Right, differentialSpeed1 < 0 ? back : just, differentialSpeed1);
        motorRun(Left, back, speed + speed * proportion1);
    } else if (yfLS.readSensor(0)) {
        motorRun(Left, differentialSpeed2 < 0 ? just : back, differentialSpeed2);
        motorRun(Right, just, speed + speed * proportion2);
    } else if (yfLS.readSensor(4)) {
        motorRun(Right, differentialSpeed2 < 0 ? back : just, differentialSpeed2);
        motorRun(Left, back, speed + speed * proportion2);
    } else {
        trolleyForward(speed);
    }
}

/**
 * @brief 巡线至十字路口停
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * */
void XYXKC_CAR::patrolToTheIntersectionAndStop(int speed, float proportion1, float proportion2) {
    while (!(yfLS.readSensor(0) && yfLS.readSensor(1) && yfLS.readSensor(2) && yfLS.readSensor(3) &&
             yfLS.readSensor(4))) {
        _PatrolLine(speed, proportion1, proportion2);

    }
    stopOfPatrolCar();

}

/**
 * @brief 巡线至左路口停
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * */
void XYXKC_CAR::patrolToTheLeftIntersectionAndStop(int speed, float proportion1, float proportion2) {
    while (!(yfLS.readSensor(0) && yfLS.readSensor(1) && yfLS.readSensor(2))) {
        _PatrolLine(speed, proportion1, proportion2);
    }
    stopOfPatrolCar();

}

/**
 * @brief 巡线至右路口停
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * */
void XYXKC_CAR::patrolToTheRightIntersectionAndStop(int speed, float proportion1, float proportion2) {
    while (!(yfLS.readSensor(3) && yfLS.readSensor(4) && yfLS.readSensor(2))) {
        _PatrolLine(speed, proportion1, proportion2);
    }
    stopOfPatrolCar();

}

/**
 * @brief 巡线至无线时停
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * */
void XYXKC_CAR::stopWhenPatrollingToWireless(int speed, float proportion1, float proportion2) {
    while ((yfLS.readSensor(0) | yfLS.readSensor(1) | yfLS.readSensor(2) | yfLS.readSensor(3) |
            yfLS.readSensor(4)) != 0) {
        _PatrolLine(speed, proportion1, proportion2);
    }
    stopOfPatrolCar();

}

/**
 * @brief 巡线至中间离线时
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * */
void XYXKC_CAR::patrolToCenterOffline(int speed, float proportion1, float proportion2) {
    while (yfLS.readSensor(2) != 0) {
        _PatrolLine(speed, proportion1, proportion2);
    }
    stopOfPatrolCar();
}

/**
 * @brief 巡线时间停
 * @param speed 速度
 * @param proportion1 调整比例1
 * @param proportion2 调整比例2
 * @param time 时间 mm
 * */
void XYXKC_CAR::linePatrolTimeStop(int speed, float proportion1, float proportion2, unsigned int time) {
    unsigned int t = millis();
    while (abs(millis() - t) < time) {
        _PatrolLine(speed, proportion1, proportion2);
    }
    stopOfPatrolCar();
}

/**
 * @brief 左转到线
 * @param speed 速度
 * @param number 巡线传感器编号
 * */
void XYXKC_CAR::turnLeftToTheLine(int speed, uint8_t number) {
    carTurningLeft(speed);
    while (yfLS.readSensor(number));
    while (!yfLS.readSensor(number));
    stopOfPatrolCar();
}

/**
 * @brief 右转到线
 * @param speed 速度
 * @param number 巡线传感器编号
 * */
void XYXKC_CAR::turnRightToTheLine(int speed, uint8_t number) {
    carTurningRight(speed);
    while (yfLS.readSensor(number));
    while (!yfLS.readSensor(number));
    stopOfPatrolCar();
}
